#include "offset.h"
#include <math.h>

float convertMod2Pi(float angle)
{
  float result = angle;
  if(angle>2*M_PI)
    {
      while(result > 2*M_PI)
	{
	  result -= 2*M_PI;
	}
    }
  if(angle < 0)
    {
      while(result<0)
	{
	  result += M_PI*2;
	}
    }
  return result;
}

Vector offsetAnglesT(Vector katana_angles)
{
  Vector result(6);
  result(0) = katana_angles(0)-M_PI;
  result(1) = -katana_angles(1);
  result(2) = M_PI - katana_angles(2);
  result(3) = katana_angles(3) - M_PI;
  result(4) = -katana_angles(4) - M_PI;
  result(5) = katana_angles(5) - 0.57;
  return result; 
}

Vector de_offsetAnglesT(Vector ik_angles)
{
  Vector result(6);
  result(0) = convertMod2Pi(ik_angles(0)+M_PI);
  result(1) = convertMod2Pi(-ik_angles(1));
  result(2) = convertMod2Pi(M_PI - ik_angles(2));
  result(3) = convertMod2Pi(ik_angles(3)+M_PI);
  result(4) = convertMod2Pi(-M_PI-ik_angles(4));
  result(5) = ik_angles(5) + 0.57;
  return result;
}

Vector offsetAnglesG(Vector katana_angles)
{
  Vector result(6);
  result(0) = katana_angles(0);
  result(1) = -katana_angles(1);
  result(2) = M_PI - katana_angles(2);
  result(3) = katana_angles(3) - M_PI;
  result(4) = -katana_angles(4);
  result(5) = katana_angles(5); // gripper joint, not used by Kinematic model
  return result;
  
}

Vector de_offsetAnglesG(Vector ik_angles)
{
  Vector result(5);
  result(0) = convertMod2Pi(ik_angles(0));
  result(1) = -ik_angles(1);
  result(2) = convertMod2Pi(M_PI - ik_angles(2));
  result(3) = convertMod2Pi(ik_angles(3)+M_PI);
  result(4) = convertMod2Pi(-ik_angles(4));
  return result;
}

Vector de_offsetAnglesG_vel(Vector ik_angles)
{
  Vector result(6);
  result(0) = ik_angles(0);
  result(1) = -ik_angles(1);
  result(2) = -ik_angles(2);
  result(3) =  ik_angles(3);
  result(4) = -ik_angles(4);
  result(5) = ik_angles(5);
  return result;
}

